#include "includes.h"
#include "vehicle.h"
#include "drivers.h"
#include "gmlan.h"

void gmlan_init(void)
{}

uint8_t gmlan_ioctl(gmlan_data *data, gmlan_recv *recv)
{
    uint8_t ret;
    uint8_t i, j;
    CanTxMsg txMsg = {
        0x00, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    CanRxMsg rxMsg;
    uint8_t index = 0;

    //prepare config package
    txMsg.StdId = data->tx_id;
    txMsg.IDE = CAN_ID_STD;
    txMsg.DLC = 8;

    //logi("tx[%04x], rx[%04x], len[%02x], service_id[%02x], cmd[%02x %02x]",
    //        data->tx_id, data->rx_id, data->len, data->service_id,
    //        data->cmd[0], data->cmd[1]);
    txMsg.Data[index ++] = data->len;
    txMsg.Data[index ++] = data->service_id;
    if(data->service_id == GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE) {
        txMsg.Data[index ++] = data->index;
        recv->offset = 0;
        txMsg.Data[index ++] = data->cmd[0];
        txMsg.Data[index ++] = data->cmd[1];
    } else if(data->service_id == GMLAN_SERVICE_ID_MODE_TWENTY_TWO) {
        recv->offset = 4;
        txMsg.Data[index ++] = data->cmd[0];
        txMsg.Data[index ++] = data->cmd[1];
    } else if(data->service_id == GMLAN_SERVICE_ID_READ_STATIC_DATA) {
        recv->offset = 3;
        txMsg.Data[index ++] = data->cmd[0];
    }
    for(i = data->len + 1; i < 8; i++) {
        txMsg.Data[i] = 0x00;
    }
    bsp_can_clear();
    bsp_can_filter(data->rx_id, data->rx_id,
            data->rx_id | 0x00ff, data->rx_id | 0x00ff);
    index = 0;
    if(vehicle_can_session(&txMsg, &rxMsg)) {
        //check response package
        if(rxMsg.Data[1] == (0x40 + data->service_id) || rxMsg.Data[0] == 0x10) {
            if(data->service_id == GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE) {
                //send start stream
                txMsg.Data[index ++] = 0x03;
                txMsg.Data[index ++] = GMLAN_SERVICE_ID_READ_DYNAMIC_DATA;
                txMsg.Data[index ++] = GMLAN_SERVICE_ID_READ_DYNAMIC_DATA_ONESHOT;
                txMsg.Data[index ++] = data->index;
                for(i = 4; i < 8; i++) {
                    txMsg.Data[i] = 0x00;
                }
                bsp_can_clear();
                bsp_can_filter(data->rx_id2, data->rx_id2,
                        data->rx_id2 | 0x00ff, data->rx_id2 | 0x00ff);
                if(vehicle_can_session(&txMsg, &rxMsg)) {
                    if(rxMsg.Data[0] == data->index) {
                        for(i = 0; i < 8; i++) {
                            recv->data[i] = rxMsg.Data[i];
                        }
                    } else {
                        loge("%s: response failed index = %02x",
                                __func__, rxMsg.Data[0]);
                        ret = FALSE;
                        goto GMLAN_END;
                    }
                } else {
                    ret = FALSE;
                    goto GMLAN_END;
                }

            } else if(data->service_id == GMLAN_SERVICE_ID_MODE_TWENTY_TWO ||
                    data->service_id == GMLAN_SERVICE_ID_READ_STATIC_DATA) {
                //TODO: Need to process long package???
                //...
                //get result
                for(i = 0; i < 8; i++) {
                    recv->data[i] = rxMsg.Data[i];
                }
            }
        } else if(rxMsg.Data[0] == 0x03 && rxMsg.Data[1] == 0x7f) {
            loge("%s: cmd limited[%02x][%02x]",
                    __func__, data->cmd[0], data->cmd[1]);
            ret = FALSE;
            goto GMLAN_END;
        } else {
            loge("%s: failed to get response package id = %02x",
                    __func__, rxMsg.Data[1]);
            ret = FALSE;
            goto GMLAN_END;
        }
    } else {
        ret = FALSE;
        goto GMLAN_END;
    }
    ret = TRUE;

GMLAN_END:
    return ret;
}

uint8_t gmlan_get_codes(gmlan_codes_cmd *cmd, gmlan_codes_result *result)
{
    uint8_t ret;
    uint16_t code;
    uint8_t i, j;
    CanTxMsg txMsg = {
        0x00, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    CanRxMsg rxMsg;
    uint8_t index = 0;
    CanTxMsg startMsg = {
        0x00, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x01, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };

    if(cmd->tx_id == 0x00)
        return FALSE;

    startMsg.StdId = cmd->tx_id;
    bsp_can_clear();
    bsp_can_filter(cmd->rx_id, cmd->rx_id,
            cmd->rx_id | 0x00ff, cmd->rx_id | 0x00ff);
    if(vehicle_can_session(&startMsg, &rxMsg)) {
        if(rxMsg.Data[1] != 0x7e) {
            loge("failed to get start command response, rx[1] = %02x",
                    rxMsg.Data[1]);
            ret = FALSE;
            goto GMLAN_CODES_END;
        }
    }
    else {
        //failed to get response
        loge("failed to get start command response");
        ret = FALSE;
        goto GMLAN_CODES_END;
    }


    //prepare config package
    txMsg.StdId = cmd->tx_id;
    txMsg.IDE = CAN_ID_STD;
    txMsg.DLC = 8;
    txMsg.Data[index++] = cmd->len;
    txMsg.Data[index++] = cmd->id1;
    txMsg.Data[index++] = cmd->id2;
    txMsg.Data[index++] = cmd->cmd;
    for(;index < 8; index++) {
        txMsg.Data[index] = 0x00;
    }
    bsp_can_clear();
    bsp_can_filter(cmd->rx_id, cmd->rx_id,
            cmd->rx_id | 0x00ff, cmd->rx_id | 0x00ff);
    //collect the recv data
    if(vehicle_can_session(&txMsg, &rxMsg)) {
        result->count = 0;
        code = (rxMsg.Data[1] << 8) | rxMsg.Data[2];
        //check return codes
        if(code == 0x0000) {
            //there is no codes
            result->count = 0;
            ret = TRUE;
            goto GMLAN_CODES_END;
        } else {
            result->codes[result->count++] = code;
            do {
                ret = vehicle_can_recv(&rxMsg);
                if(ret == TRUE) {
                    code = (rxMsg.Data[1] << 8) | rxMsg.Data[2];
                    if(code != 0x0000) {
                        result->codes[result->count++] = code;
                    }
                }
            } while(ret != FALSE && code != 0x0000);
            goto GMLAN_CODES_END;
        }

    } else {
        //failed to get response
        ret = FALSE;
        goto GMLAN_CODES_END;
    }

GMLAN_CODES_END:
    return ret;
}

